      Subroutine Interpolation(X, Y, Z, OldV, NewV)
      include "param.fi"
      include "common.fi"
      Real X, Y, Z, NewV
      Real OldV(NTOT3T)

      ! SPH Interpolation
      Call Interpolation_SPH(X, Y, Z, OldV, NewV)

      End Subroutine

C=================================================================
      Subroutine Interpolation_SPH(X, Y, Z, OldV, NewV)
      include "param.fi"
      include "common.fi"
      Real X, Y, Z, NewV, SheCoefV
      Real OldV(NTOT3T)

      Integer IB, DELI, DELJ, DELK, HII, HJJ, HKK, IB2
      Integer HI, HJ, HK, IP
      Real Dir_Distance

            NewV = 0.0
            SheCoefV = 0.0
            HII = Inbox(X - XMIN) / GridWidth + 1
            HJJ = Inbox(Y - XMIN) / GridWidth + 1
            HKK = Inbox(Z - XMIN) / GridWidth + 1

            Do DELI = -1, 1
            HI = HASHIND( HII + DELI)
            Do DELJ = -1, 1
            If (DIMM.EQ.DIMM2D .AND. DELJ.NE.0) Cycle
            HJ = HASHIND( HJJ + DELJ)
            Do DELK = -1, 1
            HK = HASHIND( HKK + DELK)
            Do IP = 1, HASSNUMS(HI,HJ,HK)
              IB2 = HASSIND(IP, HI, HJ, HK)
              Dir_Distance = Distance2P(X1(IB2), X2(IB2), X3(IB2),
     &                       X, Y, Z)
              If (Dir_Distance .LE. HSUP) Then
                SheCoefV = SheCoefV + Weight(IB2) * Kernel(Dir_Distance)
              Endif
            Enddo ! IP
            Enddo ! DELK
            Enddo ! DELJ
            Enddo ! DELI

            ! no point
            If (ABS(SheCoefV).LT.1E-8) Return

            Do DELI = -1, 1
            HI = HASHIND( HII + DELI)
            Do DELJ = -1, 1
            If (DIMM.EQ.DIMM2D .AND. DELJ.NE.0) Cycle
            HJ = HASHIND( HJJ + DELJ)
            Do DELK = -1, 1
            HK = HASHIND( HKK + DELK)
            Do IP = 1, HASSNUMS(HI,HJ,HK)
              IB2 = HASSIND(IP, HI, HJ, HK)
              Dir_Distance = Distance2P(X1(IB2), X2(IB2), X3(IB2),
     &                       X, Y, Z)
              If (Dir_Distance .LE. HSUP) Then
!              If (SHEPARD .EQ. 0) Then
!                NewV = NewV + OldV(IB2)
!     &                      * Kernel(Dir_Distance) * Weight(IB2)
!              Else
            ! FL: Always use the Shepard kernel
                NewV = NewV + OldV(IB2)
     &                      * Kernel(Dir_Distance) * Weight(IB2)
     &                      / SheCoefV
!              Endif ! Shepard
              Endif
            Enddo ! IP
            Enddo ! DELK
            Enddo ! DELJ
            Enddo ! DELI

      End Subroutine

C=================================================================
      Subroutine IntepGrad_O1(X, Z, OldV, GradX, GradZ)
      ! Only 2D version
      ! Used only with the ifDual Option
      ! 1st order
      include "param.fi"
      include "common.fi"
      Real X, Z, OldV(GridDual2), DisE, DisN, DisS, DisW
      Real vNW, vNE, vSW, vSE, G1, G2
      Real DisNW, DisNE, DisSW, DisSE, GradX, GradZ
      Integer StartI, StartJ
      
      StartI = InBox(X-XMIN) / DXDual + 1
      StartJ = InBox(Z-XMIN) / DXDual + 1

      DisW = InBox(X - XMIN) - DXDual * (StartI - 1)
      DisS = InBox(Z - XMIN) - DXDual * (StartJ - 1)
      DisE = DXDual - DisW
      DisN = DXDual - DisS
      !DisSW = Sqrt(DisW*DisW + DisS*DisS)
      !DisSE = Sqrt(DisE*DisE + DisS*DisS)
      !DisNW = Sqrt(DisW*DisW + DisN*DisN)
      !DisNE = Sqrt(DisE*DisE + DisN*DisN)

      vSW = OldV(Dual2to1(StartI, StartJ))
      vSE = OldV(Dual2to1(StartI + 1, StartJ))
      vNW = OldV(Dual2to1(StartI, StartJ + 1))
      vNE = OldV(Dual2to1(StartI + 1, StartJ + 1))

      ! Gradient X
      G1 = (vSE - vSW) / DXDual
      G2 = (vNE - vNW) / DXDual
      GradX = (G1*DisN + G2*DisS) / (DisN+DisS)
      ! GraDient Z
      G1 = (vNW - vSW) / DXDual
      G2 = (vNE - vSE) / DXDual
      GradZ = (G1*DisE + G2*DisW) / (DisE+DisW)

      End Subroutine
C=================================================================
      Subroutine Intep3P(X1,X2,X3, V1,V2,V3, XNew, VNew)
      Real X1,X2,X3, V1,V2,V3, XNew, VNew
      Real a, b, c
      a = ((X2-X1)*V3+(X1-X3)*V2+(X3-X2)*V1)
     &  / ((X2-X1)*X3*X3 + (X1*X1-X2*X2)*X3 + X1*X2*X2 - X1*X1*X2)
      b = -((X2*X2-X1*X1)*V3+(X1*X1-X3*X3)*V2+(X3*X3-X2*X2)*V1)
     &  / ((X2-X1)*X3*X3 + (X1*X1-X2*X2)*X3 + X1*X2*X2 - X1*X1*X2)
      c = ((X1*X2*X2-X1*X1*X2)*V3+(X1*X1*X3-X3*X3*X1)*V2
     &     +(X3*X3*X2-X2*X2*X3)*V1)
     &  / ((X2-X1)*X3*X3 + (X1*X1-X2*X2)*X3 + X1*X2*X2 - X1*X1*X2)
      VNew = a * XNew * XNew + b * XNew + c
      End Subroutine
C=================================================================
      Subroutine Grad3P(X1,X2,X3, V1,V2,V3, XNew, VNew)
      Real X1,X2,X3, V1,V2,V3, XNew, VNew
      Real a, b, c
      a = ((X2-X1)*V3+(X1-X3)*V2+(X3-X2)*V1)
     &  / ((X2-X1)*X3*X3 + (X1*X1-X2*X2)*X3 + X1*X2*X2 - X1*X1*X2)
      b = -((X2*X2-X1*X1)*V3+(X1*X1-X3*X3)*V2+(X3*X3-X2*X2)*V1)
     &  / ((X2-X1)*X3*X3 + (X1*X1-X2*X2)*X3 + X1*X2*X2 - X1*X1*X2)
      VNew = 2.0 * a * XNew + b 
      End Subroutine
C=================================================================
      Subroutine IntepGrad_O2(X, Z, OldV, GradX, GradZ)
      ! Only 2D version
      ! Used only with the ifDual Option
      ! 2nd order
      include "param.fi"
      include "common.fi"
      Real X, Z, OldV(GridDual2), DisE, DisN, DisS, DisW
      Real vNW, vNE, vSW, vSE, G1, G2
      Real DisNW, DisNE, DisSW, DisSE, GradX, GradZ
      Integer StartI, StartJ, I, J
      Real val(3,3), vintep(3)
      
      StartI = InBox(X - XMIN) / DXDual + 1
      StartJ = InBox(Z - XMIN) / DXDual + 1

      DisW = InBox(X - XMIN) - DXDual * (StartI - 1)
      DisS = InBox(Z - XMIN) - DXDual * (StartJ - 1)
      !DisE = DXDual - DisW
      !DisN = DXDual - DisS

      If (StartI.GT.1) Then
        StartI = StartI - 1
        DisW = DisW + DXDual
      Endif
      If (StartJ.GT.1) Then
        StartJ = StartJ - 1
        DisS = DisS + DXDual
      Endif

      !DisSW = Sqrt(DisW*DisW + DisS*DisS)
      !DisSE = Sqrt(DisE*DisE + DisS*DisS)
      !DisNW = Sqrt(DisW*DisW + DisN*DisN)
      !DisNE = Sqrt(DisE*DisE + DisN*DisN)

!      If (StartI.EQ.1 .OR. StartI.EQ.GridDual-1 .OR.
!     &    StartJ.EQ.1 .OR. StartJ.EQ.GridDual-1) Then
!        Call IntepGrad_O1(X, Z, OldV, GradX, GradZ)
!      Else
      Do I = 1, 3
      Do J = 1, 3
        val(I,J) = OldV(Dual2to1(StartI+I-1,StartJ+J-1))
      Enddo
      Enddo

      ! Gradient X
      DO I = 1, 3
        CALL Intep3P(0.0, DXDual, DXDual*2.0,
     &               val(I,1), val(I,2), val(I,3),
     &               DisS, vintep(I))
      Enddo
      CALL Grad3P (0.0, DXDual, DXDual*2.0,
     &             vintep(1), vintep(2), vintep(3),
     &             DisW, GradX)

      ! Gradient Z
      DO I = 1, 3
        CALL Intep3P(0.0, DXDual, DXDual*2.0,
     &               val(1,I), val(2,I), val(3,I),
     &               DisW, vintep(I))
      Enddo
      CALL Grad3P (0.0, DXDual, DXDual*2.0,
     &             vintep(1), vintep(2), vintep(3),
     &             DisS, GradZ)

!       Endif !Boundary points

      End Subroutine
C====================================================================
      Subroutine IntepGrad(X, Z, OldV, GradX, GradZ)
      include "param.fi"
      include "common.fi"
      Real X, Z, OldV(GridDual2), GradX, GradZ
      If (GradOrder.EQ.1) Then
        Call IntepGrad_O1(X, Z, OldV, GradX, GradZ)
      Else
        Call IntepGrad_O2(X, Z, OldV, GradX, GradZ)
      Endif
      End Subroutine
